#include <Eigen/Core>
#include <iostream>
using namespace Eigen;
using std::cout, std::endl;

// comma initializaiton

void test_comma_initialization() {
  Matrix3f A1;
  A1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  std::cout << "A1 = \n" << A1 << std::endl;
}

void test_initialization_beforecpp11() {
  // 固定尺寸，设置初始值
  Vector3d A2(1.0, 2.0, 3.0);
  RowVector4i A3(9, 8, 7, 6);
  Array3d A4(4.0, 5.0, 6.0);
  std::cout << "A2 = \n" << A2 << std::endl;
  std::cout << "A3 = \n" << A3 << std::endl;
  std::cout << "A4 = \n" << A4 << std::endl;

  // 动态尺寸，设置尺寸，初始值为0
  RowVectorXd A5(6); // row Vector with shape 1x6
  MatrixXd A6(3, 5); // matrix with shape 3x5
  ArrayXXd a7(2, 6); // array with shape 2x6
  std::cout << "A5 = \n" << A5 << std::endl;
  std::cout << "A6 = \n" << A6 << std::endl;
  std::cout << "a7 = \n" << a7 << std::endl;
}

// c++11之后，可应用列表初始化
void test_initialization_aftercpp11() {
  Matrix<double, 2, 5> A8{{1, 2, 3, 4, 5}, {6, 7, 8, 9, 0}};
  Array<int, 3, 4> a9{{1, 2, 3, 4}, {5, 6, 7, 8}, {9, 0, 1, 2}};
  MatrixXd A10{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}};
  cout << "A8 = \n" << A8 << endl;
  cout << "a9 = \n" << a9 << endl;
  cout << "A10 = \n" << A10 << endl;

  // 列向量两种初始化
  VectorXd A11{{1}, {2}, {3}};
  VectorXd A12{{1, 2, 3}};
  cout << "A11 = \n" << A11 << endl;
  cout << "A12 = \n" << A12 << endl;

  // 报错
  // RowVectorXi A13{{1}, {2}, {3}};
  // cout << "A13 = \n" << A13 << endl;
}

// 高级初始化
// - 逗号的使用
void test_comma_block() {
  RowVector3i A1, A2;
  Matrix<int, 1, 6> A3;
  Matrix<int, 2, 3> A4;
  A1 << 1, 2, 3;
  A2 << 4, 5, 6;
  A3 << A1, A2;
  A4 << A1, A2;
  cout << "A1 = \n" << A1 << endl;
  cout << "A2 = \n" << A2 << endl;
  cout << "A3 = \n" << A3 << endl;
  cout << "A4 = \n" << A4 << endl;

  Vector3i A5, A6;
  Matrix<int, 6, 1> A7;
  Matrix<int, 3, 2> A8;
  A5 << 1, 2, 3;
  A6 << 4, 5, 6;
  A7 << A5, A6;
  A8 << A5, A6;
  cout << "A5 = \n" << A5 << endl;
  cout << "A6 = \n" << A6 << endl;
  cout << "A7 = \n" << A7 << endl;
  cout << "A8 = \n" << A8 << endl;

  Matrix2i A9;      // 2x2
  Matrix4i A10;     // 4x4
  A9 << 1, 2, 3, 4; // 行优先填入
  A10 << A9, A9 * 2, A9 * 3, A9 * 4;
  cout << "A9 = \n" << A9 << endl;
  cout << "A10 = \n" << A10 << endl;

  // 报错，可以块填充块，无法块填充向量
  // Matrix<int,1,4> A11;
  // A11 << A9;
  // cout << "A11 = \n" << A11 << endl;
}

// 特殊矩阵的初始化方法
void test_special_init() {
  // Zero
  Matrix3d B1 = Matrix3d::Zero();        // 3x3
  RowVectorXd B2 = RowVectorXd::Zero(3); // 1xN 指定N=3
  ArrayXXd B3 = ArrayXXd::Zero(3, 3);    // 3x3
  cout << "B1 = \n" << B1 << endl;
  cout << "B2 = \n" << B2 << endl;
  cout << "B3 = \n" << B3 << endl;

  // Constant
  Matrix3d B4 = Matrix3d::Constant(2);          // 形状确定，值为2
  RowVectorXd B5 = RowVectorXd::Constant(6, 3); // 形状和值混合， 1x6 值为3
  ArrayXXd B6 = ArrayXXd::Constant(3, 4, 5); // 3x4 值为5
  cout << "B4 = \n" << B4 << endl;
  cout << "B5 = \n" << B5 << endl;
  cout << "B6 = \n" << B6 << endl;

  // Random 随机数：整数在整个定义范围内，浮点数在-1~1之内
  Matrix3i B7 = Matrix3i::Random();        // 3x3
  RowVectorXd B8 = RowVectorXd::Random(6); // 1xN, N=6
  ArrayXXd B9 = ArrayXXd::Random(3, 4);    // N1xN2 N1=3, N2=4
  cout << "B7 = \n" << B7 << endl;
  cout << "B8 = \n" << B8 << endl;
  cout << "B9 = \n" << B9 << endl;

  // Ones
  Matrix3d B10 = Matrix3d::Ones();        // 3x3
  RowVectorXd B11 = RowVectorXd::Ones(6); // 1xN, N=6
  ArrayXXd B12 = ArrayXXd::Ones(3, 4);    // N1xN2, N1=3, N2=4
  cout << "B10 = \n" << B10 << endl;
  cout << "B11 = \n" << B11 << endl;
  cout << "B12 = \n" << B12 << endl;

  // Identity，只适用于Matrix类，Array类不能创建单位阵
  Matrix3d B13 = Matrix3d::Identity();     // 3x3
  MatrixXd B14 = MatrixXd::Identity(4, 4); // N1xN2, N1=4, N2=4
  Matrix<double, 3, 6> B15 =
      Matrix<double, 3, 6>::Identity();    // 设置前3列是单位阵
  MatrixXd B16 = MatrixXd::Identity(4, 3); // N1xN2, N1=4, N2=3
  cout << "B13 = \n" << B13 << endl;
  cout << "B14 = \n" << B14 << endl;
  cout << "B15 = \n" << B15 << endl;
  cout << "B16 = \n" << B16 << endl;

  // LinSpaced，线性间隔向量，即等差数列
  RowVector4d B17 = RowVector4d::LinSpaced(0, 6); // 1x4, 从0到6, 即0,2,4,6
  ArrayXd B18 = ArrayXd::LinSpaced(3, 4, 8); // N1个元素，N1=3，从4到8，即4,6,8
  cout << "B17 = \n" << B17 << endl;
  cout << "B18 = \n" << B18 << endl;
}

void test_tmp_object() {
  // 如何创建C = [A, B], 其中A=[1,2,3].T B=Identity(3)
  // 一般方法1: 手动输入
  Matrix<double, 3, 4> C1;
  C1 << 1, 1, 0, 0, 2, 0, 1, 0, 3, 0, 0, 1;
  cout << "C1 = \n" << C1 << endl;

  // 一般方法2: 矩阵拼接
  Vector3d C2;
  C2 << 1, 2, 3;
  Matrix3d C3 = Matrix3d::Identity();
  Matrix<double, 3, 4> C4;
  C4 << C2, C3;
  cout << "C4 = \n" << C4 << endl;

  // 使用临时对象
  Matrix<double, 3, 4> C5;
  C5 << (Vector3d() << 1, 2, 3).finished(), Matrix3d::Identity();
  cout << "C5 = \n" << C5 << endl;

  // 其他示例
  Matrix<double, 3, 4> C6;
  C6 = (Matrix<double, 3, 4>() << (Vector3d() << 1, 2, 3).finished(),
        Matrix3d::Identity())
           .finished() +
       Matrix<double, 3, 4>::Constant(2);
  cout << "C6 = \n" << C6 << endl;

  Matrix3d C7;
  C7 = (Vector3d() << 1, 2, 3).finished() *
           (RowVector3d() << 1, 2, 3).finished() +
       Matrix3d::Identity();
  cout << "C7 = \n" << C7 << endl;
}

int main() {
  test_initialization_beforecpp11();
  test_initialization_aftercpp11();

  test_comma_initialization();
  test_comma_block();
  test_special_init();
  test_tmp_object();
  return 0;
}